/*
 * Copyright © Stéphane Raimbault <stephane.raimbault@gmail.com>
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <errno.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

#include <modbus.h>
#include "modbus_master.h"

#if defined(_WIN32)
#include <ws2tcpip.h>
#else
#include <arpa/inet.h>
#include <netinet/in.h>
#include <sys/select.h>
#include <sys/socket.h>
#endif

#define NB_CONNECTION 5

ModbusMaster::ModbusMaster(const std::shared_ptr<rclcpp::Node> node):
    ModbusCommunication(node)
{
    m_init_ok = false;
}

ModbusMaster::~ModbusMaster()
{
    stop();
    if(server_socket != 0)
    {
        close(server_socket);
        server_socket = 0;
    }
}
void ModbusMaster::start()
{
    if ( m_init_ok)
    {
      m_quit_mutex.lock();
      if (m_use_backend == TCP) {
        m_thread = std::thread( &ModbusMaster::thread_cycle_tcp, this, this);
      } else {
        m_thread = std::thread( &ModbusMaster::thread_cycle_rtu, this, this);
      }
    }
    else
    {
      RCLCPP_ERROR(rclcpp::get_logger("Modbus"), "[ModbusCommunication] m_init_ok =%d", m_init_ok);  
    }
}

void ModbusMaster::stop()
{
    m_quit_mutex.unlock();
    m_thread.join();
}

int ModbusMaster::accept_host()
{
   std::lock_guard<std::mutex> locker(m_mutex);

    if (m_use_backend == TCP) {
        m_ctx = modbus_new_tcp( m_host_ip.data(), m_host_port);
        server_socket = modbus_tcp_listen(m_ctx, NB_CONNECTION);
        if (server_socket == -1) {
            fprintf(stderr, "Unable to listen TCP connection\n");
            modbus_free(m_ctx);
            return listen_TCP_FAILED;
        }
    } else if (m_use_backend == TCP_PI) {
        m_ctx = modbus_new_tcp_pi( m_host_ip.data(), std::to_string( m_host_port).c_str());
        server_socket = modbus_tcp_pi_listen(m_ctx, NB_CONNECTION);
        if (server_socket == -1) {
            fprintf(stderr, "Unable to listen TCP connection\n");
            modbus_free(m_ctx);
            return listen_TCP_FAILED;
        }
    } else {
        m_ctx = modbus_new_rtu( m_uart.port.data(), m_uart.baudrate, *m_uart.patity.data(), m_uart.data_bits, m_uart.stop_bit);
        modbus_set_slave(m_ctx, m_slave_id);
        if (modbus_connect(m_ctx) == -1)   //等待连接设备
        {
            fprintf(stderr, "connection failed:%s\n", modbus_strerror(errno));
            return FAILED_CONNECT;
        }
    }
    modbus_set_debug(m_ctx, m_debug);

    if (m_ctx == nullptr)
    {
        fprintf(stderr, "Unable to allocate libmodbus contex\n");
        return CTX_NULLPTR;
    }

    RCLCPP_INFO(rclcpp::get_logger("Modbus"), "modbus connect success");
    return NORMALL;
}

int ModbusMaster::init(std::string prefix) 
{
    RCLCPP_INFO(rclcpp::get_logger("Modbus"), "ModbusSlave init func");
    if(m_init_ok){
        return ALREADY_INIT_ERRO;
    }
    //获取链路层参数
    get_linkpara(prefix);
    
    //MRC该端口作为主站管理器，不能在此类中初始化
    if ( !m_is_slave)    
    {
        RCLCPP_ERROR(rclcpp::get_logger("Modbus"), "the port protocol is_slave is %d,can't be init in ModbusMaster class",m_is_slave);
        return IS_SLAVE_ERRO;
    }    

    std::string temp;   
    // step4:更新从站前缀
    prefix= prefix + "/protocol/slave_para";

    temp= prefix + "/slaveID";
    nh_->get_parameter_or(temp, m_slave_id, 11);
    RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s:%d", temp.c_str(), m_slave_id);

    temp= prefix + "/number";
    nh_->get_parameter_or(temp, m_number, 10);
    RCLCPP_INFO(rclcpp::get_logger("Modbus"), "%s:%d", temp.c_str(), m_number);

    mb_mapping = modbus_mapping_new(MODBUS_MAX_WRITE_BITS, MODBUS_MAX_READ_BITS, MODBUS_MAX_WRITE_REGISTERS, MODBUS_MAX_READ_REGISTERS);
    if (mb_mapping == NULL) {
        fprintf(stderr, "Failed to allocate the mapping: %s\n", modbus_strerror(errno));
        modbus_free(m_ctx);
        return MALLOC_REG_NULL;
    } 
    m_init_ok=  true;
    return accept_host();
}
int ModbusMaster::init(std::string host_ip,int host_port,int slave_id)
{
    std::lock_guard<std::timed_mutex> locker(m_quit_mutex);
    if(m_init_ok){
        return -1;
    }
    if (host_ip.empty()){
        host_ip = "0.0.0.0";
        RCLCPP_WARN(rclcpp::get_logger("Modbus"), "empty host_ip, default: %s",host_ip.c_str());
    }
       
    if (host_port ==0){
        host_port=1502;
        RCLCPP_WARN(rclcpp::get_logger("Modbus"), "empty host_port, default: %d",host_port);
    }
    if (slave_id ==0){
        slave_id= 1;
        RCLCPP_WARN(rclcpp::get_logger("Modbus"), "empty slave_id, default: %d",slave_id);
    }
    modbus_set_debug(m_ctx, DEBUG);

    mb_mapping = modbus_mapping_new(MODBUS_MAX_WRITE_BITS, MODBUS_MAX_READ_BITS, MODBUS_MAX_WRITE_REGISTERS, MODBUS_MAX_READ_REGISTERS);
    if (mb_mapping == NULL) {
        fprintf(stderr, "Failed to allocate the mapping: %s\n", modbus_strerror(errno));
        modbus_free(m_ctx);
        return -1;
    } 
    m_init_ok=  true;
    return 0;
}

void ModbusMaster::thread_cycle_tcp(ModbusMaster *pBase)
{
    uint8_t query[MODBUS_TCP_MAX_ADU_LENGTH];
    int master_socket;
    int rc;
    fd_set refset;
    fd_set rdset;
    /* Maximum file descriptor number */
    int fdmax;
    ModbusMaster *pThis= pBase;
    /* select timeout ,check pthread whether or not need exit */
    // struct timeval tv;
    // tv.tv_sec = 5;
    // tv.tv_usec = 100;
    
    if( !m_init_ok){
        RCLCPP_ERROR(rclcpp::get_logger("Modbus"), "not init");
        return;
    }
    printf("[thread_cycle_tcp]modbus server pthead create success\n");

    /* Clear the reference set of socket */
    FD_ZERO(&refset);
    /* Add the server socket */
    FD_SET(pThis->server_socket, &refset);

    /* Keep track of the max file descriptor */
    fdmax = pThis->server_socket;
    printf("[thread_cycle_tcp]modbus server pthead blind socket success\n");

    while ( rclcpp::ok()) {
        rdset = refset;
        
        if (select(fdmax + 1, &rdset, NULL, NULL, NULL) == -1) {
            perror("Server select() failure.");
            //close_sigint(1);
        }
        //printf("select active \n");
        /* Run through the existing connections looking for data to be
         * read */
        for (master_socket = 0; master_socket <= fdmax; master_socket++) {

            if (!FD_ISSET(master_socket, &rdset)) {
                continue;
            }

            if (master_socket == pThis->server_socket) {
                /* A client is asking a new connection */
                socklen_t addrlen;
                struct sockaddr_in clientaddr;
                int newfd;

                /* Handle new connections */
                addrlen = sizeof(clientaddr);
                memset(&clientaddr, 0, sizeof(clientaddr));
                newfd = accept(pThis->server_socket, (struct sockaddr *) &clientaddr, &addrlen);
                if (newfd == -1) {
                    perror("Server accept() error");
                    
                } else {
                    FD_SET(newfd, &refset);

                    if (newfd > fdmax) {
                        /* Keep track of the maximum */
                        fdmax = newfd;
                    }
                    printf("New connection from %s:%d on socket %d\n",
                           inet_ntoa(clientaddr.sin_addr),
                           clientaddr.sin_port,
                           newfd);
                }
            } else {
                modbus_set_socket(pThis->m_ctx, master_socket);
                rc = modbus_receive(pThis->m_ctx, query);
                if (rc > 0) {
                    modbus_reply(pThis->m_ctx, query, rc, pThis->mb_mapping);
                    //printf("[thread_cycle_tcp]modbus server pthead receive msg and reply success \n");
                } else if (rc == -1) {
                    /* This example server in ended on connection closing or
                     * any errors. */
                    printf("Connection closed on socket %d\n", master_socket);
                    close(master_socket);

                    /* Remove from reference set */
                    FD_CLR(master_socket, &refset);

                    if (master_socket == fdmax) {
                        fdmax--;
                    }
                }
            }
        }
    }
    printf("modbus server listen thread exited: \n");
    //return 0;
}

void ModbusMaster::thread_cycle_rtu(ModbusMaster *pBase)
{
    uint8_t query[MODBUS_TCP_MAX_ADU_LENGTH];
    int rc;
    ModbusMaster *pThis= pBase;
    /* select timeout ,check pthread whether or not need exit */
    // struct timeval tv;
    // tv.tv_sec = 5;
    // tv.tv_usec = 100;
    
    if( !m_init_ok){
        RCLCPP_ERROR(rclcpp::get_logger("Modbus"), "not init");
        return;
    }

    while ( rclcpp::ok()) {
        rc = modbus_receive(pThis->m_ctx, query);
        if (rc > 0) {
            modbus_reply(pThis->m_ctx, query, rc, pThis->mb_mapping);

        } else if (rc == -1) {
            RCLCPP_ERROR(rclcpp::get_logger("Modbus"), "Connection closed by the client or error");
            //break;
        }

    }
    printf("modbus_rtu master thread exited: \n");
    //return 0;
}